#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
import os
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from RobotCar.CarRobot import CarRobot
from RobotManipulator.ManipulatorRobot import ManipulatorRobot
import keyboard
import time


robotlib = BwRobotLib()
carRobot = CarRobot(robotlib)
manipulator = ManipulatorRobot(robotlib)

#robotlib.mclib.SetIsDebug(True)

#连接机器人设备
robotlib.connectRobot("192.168.3.46",55001)
#robotlib.connectRobot("10.10.100.254",55000)

#配置轮式模式
print("请选择轮子模式 :")
print("2 : 两轮")
print("3 : 三轮")
print("4 : 四向轮")
print("5 : 四麦克朗姆轮")
print("6 : 四并橡胶轮")
mode = robotlib.inputNum()


#加载配置
robotlib.module_ids=robotlib.scanNConfigCarIds()

#配置机器人
#配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
carRobot.bindIds(robotlib.module_ids,mode)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)


#配置操作臂
manipulator.bindIds(robotlib.module_ids)

#读取当前坐标
manipulator.readAngles()

#-----------------------------------------------------------------------------
#小车操作臂
#-----------------------------------------------------------------------------


#运行
def runDemo():
  waitSeconds = 2
  manipulator.pickUpSample()
  carRobot.goForward()
  time.sleep(waitSeconds)
  carRobot.goBack()
  time.sleep(waitSeconds)
  carRobot.turnLeft()
  time.sleep(1)
  carRobot.turnRight()
  time.sleep(1)
  carRobot.stop()

  if(carRobot.Mode==5):
      carRobot.runLeft()
      time.sleep(waitSeconds)
      carRobot.runRight()
      time.sleep(waitSeconds)
      carRobot.stop()
  manipulator.putDownSample();

while(1):
  input("输入任意字符 Enter 运行演示示例...")
  runDemo()

